/**
    Copyright 2015 Table Rider
    By: Kristoffer Dahl
*/

#include "DistanceSensor.h"
#include "Definitions.h"
#include <stdio.h>

static int firstExe = 1;
static double dsTemp2;
static double dist;
static int ready;

DistanceSensor::DistanceSensor(int sensor)
{
    s = sensor;

    // Enable interrupt. Echo pins, PIOC bit: 4,5,6
    REG_PIOC_IER = 0x70;
    // Clear interrupt
    int clearInterrupt = REG_PIOC_ISR;

    //PIOD clear pending interrupt (PIOC = 13)
    NVIC_ClearPendingIRQ((IRQn_Type)13);
    //PIOD set priority for interrupt
    NVIC_SetPriority((IRQn_Type)13, 4);
    //PIOD enable interrupt
    NVIC_EnableIRQ((IRQn_Type)13);

    //select Timer_Clock2 MCK/8
    REG_TC1_CMR0 = 0x1;

    // Config ports for trigger, PIOC bit 1,2,3
    REG_PIOC_PER = 0xE;
    REG_PIOC_OER = 0xE;//ctor
    REG_PIOC_CODR = 0xE;
}

unsigned long DistanceSensor::measureDistance(int id)
{
    //trigger pulse

    if(firstExe == 1)
    {
    int clearInterrupt = REG_PIOC_ISR;
        NVIC_ClearPendingIRQ((IRQn_Type)13);
        ready = 0;
        //Trigger pulse ca 10 uS
        REG_PIOC_SODR = 1<<id;
        //*REG_PIOC_SODR = 2;
        unsigned int old = micros();
        while(micros()-old < 10) asm("nop");
        //delay(90);
        //*REG_PIOC_CODR = 2;
        REG_PIOC_CODR = 1<<id;
        //Set interrupt on rising edge
        REG_PIOC_ESR = 0x70;
        REG_PIOC_REHLSR = 0x70;
        // Enable Interrupt
        REG_PIOC_IER = 0x70;
    }
    return s;
}
int DistanceSensor::isReady()
{
    return ready;
}
double DistanceSensor::getDistance()
{
	ready = 0;
    return dist;
}
DistanceSensor::~DistanceSensor()
{
    //dtor
}

extern "C" {
  //#include "../../system/CMSIS/Device/ATMEL/sam3xa/include/sam3x8e.h"
#include "../../system/CMSIS/Device/ATMEL/sam3xa/include/system_sam3xa.h"
void PIOC_Handler(void)
{
    //Disable Interrupt
    REG_PIOC_IDR = 0x70;
    //Clear interrupt
    int clearInterrupt = REG_PIOC_ISR;
    if(firstExe == 1)
    {
        firstExe = 0;
        //Reset and start TC1 timer
        REG_TC1_CCR0 = 0x5;
        //Set PIOC_interrupt to falling edge
        REG_PIOC_FELLSR = 0x70;
        // Enable interrupt
        REG_PIOC_IER = 0x70;

    }
    else if(firstExe == 0)
    {
        // Time the echo is HIGH. /10.5 for uS
        dsTemp2 = (double)(REG_TC1_CV0)/10.5/58.0;
        REG_TC1_CCR0 = 0x1;
        // Echotime/10.5 for uS. /58 to centimeters

        /*if(dsTemp2 < 400)
        {
            dist = dsTemp2;
            //printf("%f\n", dsTemp2);
        }*/
        dist = dsTemp2;
        firstExe = 1;
        ready = 1;

    }
    NVIC_ClearPendingIRQ((IRQn_Type)13);
}
}

